# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Eric Perko
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the names of the authors nor the names of their
#    affiliated organizations may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import math

import rclpy

from rclpy.node import Node
from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference
from std_msgs.msg import Int32
from geometry_msgs.msg import TwistStamped, QuaternionStamped
from tf_transformations import quaternion_from_euler
from libnmea_navsat_driver.checksum_utils import check_nmea_checksum
from libnmea_navsat_driver import parser
import numpy as np




class GPSFilter:
    '''
    连续几帧 GPS 数据进行采样，取它们的平均值
    可调的样本大小（sample_size）可以根据需要调整平滑程度
    '''
    def __init__(self, sample_size=10):
        # 存储最近的 GPS 样本
        self.samples = []
        self.sample_size = sample_size

    def add_sample(self, latitude, longitude):
        # 添加新样本
        self.samples.append((latitude, longitude))
        
        # 保持样本数量不超过 sample_size
        if len(self.samples) > self.sample_size:
            self.samples.pop(0)

    def get_average_position(self):
        # 计算样本的平均位置
        if len(self.samples) == 0:
            return None  # 没有足够数据
        
        avg_latitude = sum([s[0] for s in self.samples]) / len(self.samples)
        avg_longitude = sum([s[1] for s in self.samples]) / len(self.samples)
        return avg_latitude, avg_longitude


class GPSFilter2:
    def __init__(self, window_size=10):
        self.window_size = window_size
        self.gps_data = []

    def filter(self, latitude,longitude):
        # 保存历史 GPS 数据
        self.gps_data.append((latitude, longitude))
        
        # 只保留最新的 window_size 个数据点
        if len(self.gps_data) > self.window_size:
            self.gps_data.pop(0)
        
        # 计算滑动平均
        avg_lat = np.mean([data[0] for data in self.gps_data])
        avg_lon = np.mean([data[1] for data in self.gps_data])
        
        
        # 返回平滑后的数据
        # latitude = avg_lat
        # longitude = avg_lon
        
        
        return avg_lat,avg_lon


class KalmanFilter:
    '''
    在实际应用中，过程噪声 (process_variance) 和 测量噪声 (measurement_variance) 需要调整。
    这些参数影响滤波器对跳变或噪声的响应：
    过程噪声: 代表系统内部的变化。如果设置较大，滤波器对变化敏感，响应快，但可能对噪声不够鲁棒。
    测量噪声: 代表 GPS 测量中的噪声。这个值越大，滤波器对测量值的信任度越低
    '''
    def __init__(self, process_variance, measurement_variance):
        # 初始化滤波器的状态
        self.estimate = 0  # 位置估计
        self.error_estimate = 1  # 估计误差
        self.process_variance = process_variance  # 系统的过程噪声
        self.measurement_variance = measurement_variance  # 测量噪声
        self.kalman_gain = 0

    def update(self, measurement):
        # 预测更新
        self.error_estimate += self.process_variance
        
        # 计算卡尔曼增益
        self.kalman_gain = self.error_estimate / (self.error_estimate + self.measurement_variance)
        
        # 更新估计值
        self.estimate += self.kalman_gain * (measurement - self.estimate)
        
        # 更新误差估计
        self.error_estimate *= (1 - self.kalman_gain)
        
        return self.estimate

class GPSKalmanFilter:
    def __init__(self):
        # 针对纬度和经度分别创建卡尔曼滤波器
        self.kalman_filter_lat = KalmanFilter(process_variance=1e-4, measurement_variance=0.1)
        self.kalman_filter_lon = KalmanFilter(process_variance=1e-4, measurement_variance=0.1)

    def update_latitude(self, latitude):
        # 使用卡尔曼滤波分别更新纬度和经度
        filtered_latitude = self.kalman_filter_lat.update(latitude)
        return filtered_latitude
    def update_longitude(self,longitude):
        # 使用卡尔曼滤波分别更新纬度和经度
        filtered_longitude = self.kalman_filter_lon.update(longitude)
        return filtered_longitude



class Ros2NMEADriver(Node):
    def __init__(self):
        super().__init__('nmea_navsat_driver')
        self.gps_kalman_filter = GPSKalmanFilter()
        self.gps_filter = GPSFilter2()

        self.fix_pub = self.create_publisher(NavSatFix, 'fix', 10)
        self.vel_pub = self.create_publisher(TwistStamped, 'vel', 10)
        self.heading_pub = self.create_publisher(QuaternionStamped, 'heading', 10)
        self.time_ref_pub = self.create_publisher(TimeReference, 'time_reference', 10)
        self.fix_type_pub = self.create_publisher(Int32,'fix_type',1)

        self.time_ref_source = self.declare_parameter('time_ref_source', 'gps').value
        self.use_RMC = self.declare_parameter('useRMC', False).value
        self.valid_fix = False

        # epe = estimated position error
        self.default_epe_quality0 = self.declare_parameter('epe_quality0', 1000000).value
        self.default_epe_quality1 = self.declare_parameter('epe_quality1', 4.0).value
        self.default_epe_quality2 = self.declare_parameter('epe_quality2', 0.1).value
        self.default_epe_quality4 = self.declare_parameter('epe_quality4', 0.02).value
        self.default_epe_quality5 = self.declare_parameter('epe_quality5', 4.0).value
        self.default_epe_quality9 = self.declare_parameter('epe_quality9', 3.0).value

        self.using_receiver_epe = False

        self.lon_std_dev = float("nan")
        self.lat_std_dev = float("nan")
        self.alt_std_dev = float("nan")

        """Format for this dictionary is the fix type from a GGA message as the key, with
        each entry containing a tuple consisting of a default estimated
        position error, a NavSatStatus value, and a NavSatFix covariance value."""
        self.gps_qualities = {
            # Unknown
            -1: [
                self.default_epe_quality0,
                NavSatStatus.STATUS_NO_FIX,
                NavSatFix.COVARIANCE_TYPE_UNKNOWN
            ],
            # Invalid
            0: [
                self.default_epe_quality0,
                NavSatStatus.STATUS_NO_FIX,
                NavSatFix.COVARIANCE_TYPE_UNKNOWN
            ],
            # SPS
            1: [
                self.default_epe_quality1,
                NavSatStatus.STATUS_FIX,
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
            ],
            # DGPS
            2: [
                self.default_epe_quality2,
                NavSatStatus.STATUS_SBAS_FIX,
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
            ],
            # RTK Fix
            4: [
                self.default_epe_quality4,
                NavSatStatus.STATUS_GBAS_FIX,
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
            ],
            # RTK Float
            5: [
                self.default_epe_quality5,
                NavSatStatus.STATUS_GBAS_FIX,
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
            ],
            # WAAS
            9: [
                self.default_epe_quality9,
                NavSatStatus.STATUS_GBAS_FIX,
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
            ]
        }

    # Returns True if we successfully did something with the passed in
    # nmea_string
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        if not check_nmea_checksum(nmea_string):
            self.get_logger().warn("Received a sentence with an invalid checksum. " +
                                   "Sentence was: %s" % nmea_string)
            return False

        parsed_sentence = parser.parse_nmea_sentence(nmea_string)
        if not parsed_sentence:
            self.get_logger().debug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = self.get_clock().now().to_msg()

        current_fix = NavSatFix()
        current_fix.header.stamp = current_time
        current_fix.header.frame_id = frame_id
        current_time_ref = TimeReference()
        current_time_ref.header.stamp = current_time
        current_time_ref.header.frame_id = frame_id
        if self.time_ref_source:
            current_time_ref.source = self.time_ref_source
        else:
            current_time_ref.source = frame_id

        if not self.use_RMC and 'GGA' in parsed_sentence:
            # print(parsed_sentence)
            current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED

            data = parsed_sentence['GGA']
            fix_type = data['fix_type']
            if not (fix_type in self.gps_qualities):
                fix_type = -1
            gps_qual = self.gps_qualities[fix_type]
            default_epe = gps_qual[0]
            current_fix.status.status = gps_qual[1]
            current_fix.position_covariance_type = gps_qual[2]
            if current_fix.status.status > 0:
                self.valid_fix = True
            else:
                self.valid_fix = False

            current_fix.status.service = NavSatStatus.SERVICE_GPS
            latitude = data['latitude']
            if data['latitude_direction'] == 'S':
                latitude = -latitude            
            current_fix.latitude = latitude # 纬度
            # 滤波
            # filtered_latitude = self.gps_kalman_filter.update_latitude(latitude)
            # print(type(filtered_latitude))
            # 留7位有效数 厘米级误差
#            current_fix.latitude = round(latitude, 7)

            longitude = data['longitude']
            if data['longitude_direction'] == 'W':
                longitude = -longitude
            current_fix.longitude = longitude # 经度
            # 滤波
            # filtered_longitude = self.gps_kalman_filter.update_longitude(longitude)
            # 保留6位有效数
 #           current_fix.longitude = round(longitude, 7)
            # Altitude is above ellipsoid, so adjust for mean-sea-level
            altitude = data['altitude'] + data['mean_sea_level']
            current_fix.altitude = altitude # 海拔高度

            # 使用 低速滤波器：滑动平均滤波器
            current_fix.latitude,current_fix.longitude = self.gps_filter.filter(current_fix.latitude,current_fix.longitude)

            # use default epe std_dev unless we've received a GST sentence with epes
            if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
                self.lon_std_dev = default_epe
            if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
                self.lat_std_dev = default_epe
            if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
                self.alt_std_dev = default_epe * 2

            hdop = data['hdop']
            current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
            current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
            current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2  # FIXME


            self.fix_pub.publish(current_fix)
            # 创建一个Int32消息对象
            fix_type_msg = Int32()
            # 将整数值赋给消息对象的data属性
            fix_type_msg.data = fix_type
            self.fix_type_pub.publish(fix_type_msg)

            if not math.isnan(data['utc_time']):
                current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
                self.last_valid_fix_time = current_time_ref
                self.time_ref_pub.publish(current_time_ref)

        elif not self.use_RMC and 'VTG' in parsed_sentence:
            data = parsed_sentence['VTG']

            # Only report VTG data when you've received a valid GGA fix as well.
            if self.valid_fix:
                current_vel = TwistStamped()
                current_vel.header.stamp = current_time
                current_vel.header.frame_id = frame_id
                current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course'])
                current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course'])
                self.vel_pub.publish(current_vel)

        # elif 'RMC' in parsed_sentence:
        #     data = parsed_sentence['RMC']
        
        #     # Only publish a fix from RMC if the use_RMC flag is set.
        #     if self.use_RMC:
        #         if data['fix_valid']:
        #             current_fix.status.status = NavSatStatus.STATUS_FIX
        #         else:
        #             current_fix.status.status = NavSatStatus.STATUS_NO_FIX

        #         current_fix.status.service = NavSatStatus.SERVICE_GPS

        #         latitude = data['latitude']
        #         if data['latitude_direction'] == 'S':
        #             latitude = -latitude
        #         current_fix.latitude = latitude

        #         longitude = data['longitude']
        #         if data['longitude_direction'] == 'W':
        #             longitude = -longitude
        #         current_fix.longitude = longitude

        #         current_fix.altitude = float('NaN')
        #         current_fix.position_covariance_type = \
        #             NavSatFix.COVARIANCE_TYPE_UNKNOWN

        #         self.fix_pub.publish(current_fix)

        #         if not math.isnan(data['utc_time']):
        #             current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
        #             self.time_ref_pub.publish(current_time_ref)

        #     # Publish velocity from RMC regardless, since GGA doesn't provide it.
        #     if data['fix_valid']:
        #         current_vel = TwistStamped()
        #         current_vel.header.stamp = current_time
        #         current_vel.header.frame_id = frame_id
        #         current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course'])
        #         current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course'])
        #         self.vel_pub.publish(current_vel)

        elif 'RMC' in parsed_sentence:
            data = parsed_sentence['RMC']
                  

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                current_vel = TwistStamped()
                current_vel.header.stamp = current_time
                current_vel.header.frame_id = frame_id
                current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course'])
                current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course'])
                self.vel_pub.publish(current_vel)

                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, data['true_course'])
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)


        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)
        else:
            return False
        return True

    """Helper method for getting the frame_id with the correct TF prefix"""
    def get_frame_id(self):
        frame_id = self.declare_parameter('frame_id', 'gps').value
        prefix = self.declare_parameter('tf_prefix', '').value
        if len(prefix):
            return '%s/%s' % (prefix, frame_id)
        return frame_id
